/*
 * servo.c
 *
 *  Created on: May 27, 2021
 *      Author: kirk_
 */

#include "servo.h"

void Servo_Turn(uint8_t index, uint8_t angle)
{
	float ftmp = angle;
	ftmp -= 90.00;
	ftmp /= 90.00;
	KDL_Servo_Set(&Servo[index>3?0:index], ftmp);
	return;
}
